#!/usr/bin/env python3
# -*- encoding: utf-8 -*-
'''
@File         :move_gazebo_model.py
@Description  :
@Time         :2024/08/01 14:11:45
@Author       :Lin Yuheng
@Version      :1.0
'''

import rospy
from gazebo_msgs.srv import SetModelState
from gazebo_msgs.msg import ModelState
from geometry_msgs.msg import Pose,Twist
from geographic_msgs.msg import GeoPointStamped
from sensor_msgs.msg import NavSatFix
from mavros_msgs.msg import State, HomePosition
from mavros_msgs.srv import CommandHome, CommandHomeRequest


leader_home_position = HomePosition()
mavros_state = State()

def mavros_state_cb(msg=State):
    global mavros_state
    mavros_state = msg

def leader_home_position_cb(msg = HomePosition):
    global leader_home_position
    leader_home_position = msg
    print(leader_home_position)

def set_gp_origin():

    rospy.init_node('set_gp_origin_node')
    rospy.wait_for_service('/plane_1/mavros/cmd/set_home')
    
    leader_home_pose_sub = rospy.Subscriber('/plane_0/mavros/home_position/home', HomePosition, callback=leader_home_position_cb)
    mavros_state_sub = rospy.Subscriber('/plane_1/mavros/state', State, callback=mavros_state_cb)
    # set_home_pose_pub = rospy.Publisher('/plane_1/mavros/home_position/set', HomePosition, queue_size=10)
    set_home_pose_srv = rospy.ServiceProxy('/plane_1/mavros/cmd/set_home', CommandHome)

    # global_home_pose = HomePosition()
    # 位置
    # global_home_pose.geo.latitude = leader_home_position.geo.latitude
    # global_home_pose.geo.longitude = leader_home_position.geo.longitude
    # global_home_pose.geo.altitude = leader_home_position.geo.altitude
    
    # print(leader_home_position)
    global_home_pose = CommandHomeRequest()
    global_home_pose.latitude = leader_home_position.geo.latitude
    global_home_pose.longitude = leader_home_position.geo.longitude
    global_home_pose.altitude = leader_home_position.geo.altitude
    global_home_pose.current_gps = True

    # Call the service
    while not rospy.is_shutdown():
        print(leader_home_position)
        # set_home_pose_pub.publish(global_pose_origin)
        # rospy.loginfo("Set new home position")
        
        resp = set_home_pose_srv(global_home_pose)
        if resp.success:
            rospy.loginfo("Home position set successfully")
            break
        else:
            rospy.logerr("Failed to set home position")
        
        rospy.sleep(0.02)
        
if __name__ == '__main__':
    try:
        set_gp_origin()
    except rospy.ROSInterruptException:
        pass